package away3d.core.math ;


import flash.geom.Matrix3D ;
import flash.geom.Orientation3D ;
import flash.geom.Vector3D ;



/**
* A Quaternion object which can be used to represent rotations.
*/
class Quaternion
{
    
    
    /**
     * The x value of the quaternion.
     */
    public var x:   Float ;
    
    /**
     * The y value of the quaternion.
     */
    public var y:   Float ;
    
    /**
     * The z value of the quaternion.
     */
    public var z:   Float ;
    
    /**
     * The w value of the quaternion.
     */
    public var w:   Float ;
    
    
    
    /**
     * Creates a new Quaternion object.
     * @param x The x value of the quaternion.
     * @param y The y value of the quaternion.
     * @param z The z value of the quaternion.
     * @param w The w value of the quaternion.
     */
    public function Quaternion( ?x_ : Float = 0, ?y_ : Float = 0, ?z_ : Float = 0, ?w_ : Float = 1 )
    {
        
        x = x_ ;
        y = y_ ;
        z = z_ ;
        w = w_ ;
        
    }
    
    
    /**
    * Returns the magnitude of the quaternion object.
    */
    private function get_magnitude() : Float
    {
        
        return Math.sqrt( w*w + x*x + y*y + z*z ) ;
        
    }
    
    
    /**
    * Fills the quaternion object with the result from a multipication of two quaternion objects.
    * 
    * @param    qa    The first quaternion in the multipication.
    * @param    qb    The second quaternion in the multipication.
    */
    public function multiply( qa: Quaternion, qb: Quaternion )
    {
        
        var w1 = qa.w ;
        var x1 = qa.x ;
        var y1 = qa.y ;
        var z1 = qa.z ;
        var w2 = qb.w ;
        var x2 = qb.x ;
        var y2 = qb.y ;
        var z2 = qb.z ;
        
        w = w1*w2 - x1*x2 - y1*y2 - z1*z2 ;
        x = w1*x2 + x1*w2 + y1*z2 - z1*y2 ;
        y = w1*y2 - x1*z2 + y1*w2 + z1*x2 ;
        z = w1*z2 + x1*y2 - y1*x2 + z1*w2 ;
        
    }
    
    
    public function multiplyVector( vector : Vector3D, target : Quaternion = null ) : Quaternion
    {
        target ||= new Quaternion() ;

        var x2 : Float = vector.x ;
        var y2 : Float = vector.y ;
        var z2 : Float = vector.z ;

        target.w = - x*x2 - y*y2 - z*z2 ;
        target.x = w*x2 + y*z2 - z*y2 ;
        target.y = w*y2 - x*z2 + z*x2 ;
        target.z = w*z2 + x*y2 - y*x2 ;

        return target ;
    }
    
    /**
     * Fills the quaternion object with values representing the given rotation around a vector.
     * 
     * @param    x        The x value of the rotation vector.
     * @param    y        The y value of the rotation vector.
     * @param    z        The z value of the rotation vector.
     * @param    angle    The angle in radians of the rotation.
     */
    public function fromAxisAngle( axis : Vector3D, angle:Float )
    {
        
        var sin_a   = Math.sin( angle / 2 ) ;
        var cos_a   = Math.cos( angle / 2 ) ;
        x           = axis.x*sin_a ;
        y           = axis.y*sin_a ;
        z           = axis.z*sin_a ;
        w           = cos_a ;
        
        normalize() ;
        
    }
    
    
    /**
     * Spherically interpolates between two quaternions, providing an interpolation between rotations with constant angle change rate.
     * @param qa The first quaternion to interpolate.
     * @param qb The second quaternion to interpolate.
     * @param t The interpolation weight, a value between 0 and 1.
     */
    public function slerp( qa : Quaternion, qb : Quaternion, t : Float )
    {
        
        var w1  = qa.w ;
        var x1  = qa.x ;
        var y1  = qa.y ;
        var z1  = qa.z ;
        var w2  = qb.w ;
        var x2  = qb.x ;
        var y2  = qb.y ;
        var z2  = qb.z ;
        var dot = w1*w2 + x1*x2 + y1*y2 + z1*z2 ;
        
        // shortest direction
        if( dot < 0 ) 
        {
            
            dot = -dot ;
            w2  = -w2 ;
            x2  = -x2 ;
            y2  = -y2 ;
            z2  = -z2 ;
            
        }
        
        if( dot < 0.95 )
        {
            // interpolate angle linearly
            var angle   = Math.acos( dot ) ;
            var s       = 1/Math.sin( angle ) ;
            var s1      = Math.sin( angle*( 1 - t ) )*s ;
            var s2      = Math.sin( angle*t )*s ;
            w           = w1*s1 + w2*s2 ;
            x           = x1*s1 + x2*s2 ;
            y           = y1*s1 + y2*s2 ;
            z           = z1*s1 + z2*s2 ;
            
        }
        else 
        {
            
            // nearly identical angle, interpolate linearly
            w       = w1 + t*( w2 - w1 ) ;
            x       = x1 + t*( x2 - x1 ) ;
            y       = y1 + t*( y2 - y1 ) ;
            z       = z1 + t*( z2 - z1 ) ;
            var len = 1.0/Math.sqrt( w*w + x*x + y*y + z*z ) ;
            w       *= len ;
            x       *= len ;
            y       *= len ;
            z       *= len ;
            
        }
        
    }
    
    
    /**
     * Linearly interpolates between two quaternions.
     * @param qa The first quaternion to interpolate.
     * @param qb The second quaternion to interpolate.
     * @param t The interpolation weight, a value between 0 and 1.
     */
    public function lerp( qa : Quaternion, qb : Quaternion, t : Float )
    {
        
        var w1  = qa.w ;
        var x1  = qa.x ;
        var y1  = qa.y ;
        var z1  = qa.z ;
        var w2  = qb.w ;
        var x2  = qb.x ;
        var y2  = qb.y ;
        var z2  = qb.z ;
        var len : Float ;
        
        // shortest direction
        if( w1*w2 + x1*x2 + y1*y2 + z1*z2 < 0 )
        {
            
            w2 = -w2 ;
            x2 = -x2 ;
            y2 = -y2 ;
            z2 = -z2 ;
            
        }
        
        w   = w1 + t*( w2 - w1 ) ;
        x   = x1 + t*( x2 - x1 ) ;
        y   = y1 + t*( y2 - y1 ) ;
        z   = z1 + t*( z2 - z1 ) ;
        
        len = 1.0/Math.sqrt( w*w + x*x + y*y + z*z ) ;
        w   *= len ;
        x   *= len ;
        y   *= len ;
        z   *= len ;
        
    }
    
    
    /**
     * Fills the quaternion object with values representing the given euler rotation.
     *
     * @param    ax        The angle in radians of the rotation around the x axis.
     * @param    ay        The angle in radians of the rotation around the y axis.
     * @param    az        The angle in radians of the rotation around the z axis.
     */
    public function fromEulerAngles( ax: Float, ay: Float, az: Float )
    {
        var fSinPitch       = Math.sin( ax * 0.5 ) ;
        var fCosPitch       = Math.cos( ax * 0.5 ) ;
        var fSinYaw         = Math.sin( ay * 0.5 ) ;
        var fCosYaw         = Math.cos( ay * 0.5 ) ;
        var fSinRoll        = Math.sin( az * 0.5 ) ;
        var fCosRoll        = Math.cos( az * 0.5 ) ;
        var fCosPitchCosYaw = fCosPitch * fCosYaw ;
        var fSinPitchSinYaw = fSinPitch * fSinYaw ;
        x                   = fSinRoll * fCosPitchCosYaw     - fCosRoll * fSinPitchSinYaw ;
        y                   = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw ;
        z                   = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw ;
        w                   = fCosRoll * fCosPitchCosYaw     + fSinRoll * fSinPitchSinYaw ;
        
    }
    
    
    /**
     * Fills a target Vector3D object with the Euler angles that form the rotation represented by this quaternion.
     * @param target An optional Vector3D object to contain the Euler angles. If not provided, a new object is created.
     * @return The Vector3D containing the Euler angles.
     */
    public function toEulerAngles( ?target : Vector3D = null ) : Vector3D
    {
        
        if( target == null )
        {
            
            target = new Vector3D() ;
            
        }
        
        target.x = Math.atan2( 2*( w*x + y*z ), 1-2*( x*x+y*y ) ) ;
        target.y = Math.asin( 2*( w*y - z*x ) ) ;
        target.z = Math.atan2( 2*( w*z + x*y ), 1-2*( y*y+z*z ) ) ;
        
        return target ;
        
    }
    
    
    /**
    * Normalises the quaternion object.
    */
    public function normalize( ?val: Float = 1 )
    {
        
        var mag = val/Math.sqrt( x*x + y*y + z*z + w*w ) ;
        x       *= mag ;
        y       *= mag ;
        z       *= mag ;
        w       *= mag ;
        
    }
    
    
    /**
     * Used to trace the values of a quaternion.
     *
     * @return A string representation of the quaternion object.
     */
    public function toString(): String
    {
        
        return "{x:" + x + " y:" + y + " z:" + z + " w:" + w + "}" ;
        
    }
    
    
    /**
     * Converts the quaternion to a Matrix3D object representing an equivalent rotation.
     * @param target An optional Matrix3D container to store the transformation in. If not provided, a new object is created.
     * @return A Matrix3D object representing an equivalent rotation.
     */
    public function toMatrix3D( ?target : Matrix3D = null ) : Matrix3D
    {
        
        var rawData     = Matrix3DUtils.RAW_DATA_CONTAINER ;
        var xy2         = 2.0*x*y ;
        var xz2         = 2.0*x*z ;
        var xw2         = 2.0*x*w ;
        var yz2         = 2.0*y*z ;
        var yw2         = 2.0*y*w ;
        var zw2         = 2.0*z*w ;
        var xx          = x*x ;
        var yy          = y*y ;
        var zz          = z*z ;
        var ww          = w*w ;
        
        rawData[ 0  ]   = xx - yy - zz + ww ;
        rawData[ 4  ]   = xy2 - zw2 ;
        rawData[ 8  ]   = xz2 + yw2 ;
        rawData[ 12 ]   = 0 ;
        rawData[ 1  ]   = xy2 + zw2 ;
        rawData[ 5  ]   = -xx + yy - zz + ww ;
        rawData[ 9  ]   = yz2 - xw2 ;
        rawData[ 13 ]   = 0 ;
        rawData[ 2  ]   = xz2 - yw2 ;
        rawData[ 6  ]   = yz2 + xw2 ;
        rawData[ 10 ]   = -xx - yy + zz + ww ;
        rawData[ 14 ]   = 0 ;
        rawData[ 3  ]   = 0.0 ;
        rawData[ 7  ]   = 0.0 ;
        rawData[ 11 ]   = 0 ;
        rawData[ 15 ]   = 1 ;
        
        if( !target ) return new Matrix3D( rawData ) ;
        
        target.copyRawDataFrom( rawData ) ;
        
        return target ;
        
    }
    
    
    /**
     * Extracts a quaternion rotation matrix out of a given Matrix3D object.
     * @param matrix The Matrix3D out of which the rotation will be extracted.
     */
    public function fromMatrix( matrix : Matrix3D )
    {
        
        var v   = matrix.decompose( Orientation3D.QUATERNION )[ 1 ] ;
        x       = v.x ;
        y       = v.y ;
        z       = v.z ;
        w       = v.w ;
        
    }
    
    
    /**
     * Converts the quaternion to a Vector<Float> matrix representation of a rotation equivalent to this quaternion.
     * @param target The Vector<Float> to contain the raw matrix data.
     * @param exclude4thRow If true, the last row will be omitted, and a 4x3 matrix will be generated instead of a 4x4.
     */
    public function toRawData( target : Vector<Float>, exclude4thRow : Bool = false )
    {
        
        var xy2 = 2.0*x*y ;
        var xz2 = 2.0*x*z ;
        var xw2 = 2.0*x*w ;
        var yz2 = 2.0*y*z ;
        var yw2 = 2.0*y*w ;
        var zw2 = 2.0*z*w ;
        var xx  = x*x ;
        var yy  = y*y ;
        var zz  = z*z ;
        var ww  = w*w ;
        
        target[ 0  ] = xx - yy - zz + ww ;
        target[ 1  ] = xy2 - zw2 ;
        target[ 2  ] = xz2 + yw2 ;
        target[ 4  ] = xy2 + zw2 ;
        target[ 5  ] = -xx + yy - zz + ww ;
        target[ 6  ] = yz2 - xw2 ;
        target[ 8  ] = xz2 - yw2 ;
        target[ 9  ] = yz2 + xw2 ;
        target[ 10 ] = -xx - yy + zz + ww ;
        target[ 3  ] = 0 ;
        target[ 7  ] = 0
        target[ 11 ] = 0 ;
        
        if( !exclude4thRow )
        {
            
            target[ 12 ] = 0 ;
            target[ 13 ] = 0 ;
            target[ 14 ] = 0 ;
            target[ 15 ] = 1 ;
            
        }
        
    }
    
    
    /**
     * Clones the quaternion.
     * @return An exact duplicate of the current Quaternion.
     */
    public function clone() : Quaternion
    {
        
        return new Quaternion( x, y, z, w ) ;
        
    }
    
    
    /**
     * Rotates a point.
     * @param vector The Vector3D object to be rotated.
     * @param target An optional Vector3D object that will contain the rotated coordinates. If not provided, a new object will be created.
     * @return A Vector3D object containing the rotated point.
     */
    public function rotatePoint( vector : Vector3D, ?target : Vector3D = null ) : Vector3D
    {
        
        var x1 : Float ;
        var y1 : Float ;
        var z1 : Float ;
        var w1 : Float ;
        var x2 = vector.x ;
        var y2 = vector.y ;
        var z2 = vector.z ;
        
        if( target == null )
        {
            
            target = new Vector3D() ;
            
        }
        
        // p*q'
        w1          = - x*x2 - y*y2 - z*z2 ;
        x1          = w*x2 + y*z2 - z*y2 ;
        y1          = w*y2 - x*z2 + z*x2 ;
        z1          = w*z2 + x*y2 - y*x2 ;
        target.x    = -w1*x + x1*w - y1*z + z1*y ;
        target.y    = -w1*y + x1*z + y1*w - z1*x ;
        target.z    = -w1*z - x1*y + y1*x + z1*w ;
        
        return target ;
        
    }
    
    
    /**
     * Copies the data from a quaternion into this instance.
     * @param q The quaternion to copy from.
     */
    public function copyFrom( q : Quaternion )
    {
        
        x = q.x ;
        y = q.y ;
        z = q.z ;
        w = q.w ;
        
    }
    
    
}